#ifndef ULTRASONIC_DRIVER_H
#define ULTRASONIC_DRIVER_H 

//#define ULTRASONIC_DEBUG

#include <iostream>
#include <thread>
#include <memory.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Range.h>
#include "serial_port.h"

namespace PandaUGV {

class UltrasonicDriver {
public:
    UltrasonicDriver();
    ~UltrasonicDriver();

public:

private:
    void connect(std::string dev_name, uint32_t bouadrate);
    void sendData();
    void readData();
    void processRxData(uint8_t data);
    void sensor_msg_init(sensor_msgs::Range &range_name, const char *frame_id_name);

private:
    std::shared_ptr<PandaUGV::SerialPort> port_;
    std::shared_ptr<std::thread> read_data_thread_;
    std::shared_ptr<std::thread> send_data_thread_;
    ros::Publisher us_l_publisher;
    ros::Publisher us_c1_publisher;
    ros::Publisher us_c2_publisher;
    ros::Publisher us_r_publisher;
    int distance_data[4];
    bool bFinished;

    sensor_msgs::Range range_left;
    sensor_msgs::Range range_center1;
    sensor_msgs::Range range_center2;
    sensor_msgs::Range range_right;
};

enum ultrasonic_status_enum{
    US_HEADER = 0,
    US_DATA1_HIGH,
    US_DATA1_LOW,
    US_DATA2_HIGH,
    US_DATA2_LOW,
    US_DATA3_HIGH,
    US_DATA3_LOW,
    US_DATA4_HIGH,
    US_DATA4_LOW,
    US_CHECKSUM,
};

}

#endif // PANDA_DRIVER_H
